/********************************************************************************

 **** Copyright (C), 2018, Fortior Technology Co., Ltd.                      ****

 ********************************************************************************
 * File Name     : FOCTailDect.c
 * Author        : Bruce HW&RD
 * Date          : 2018-12-27
 * Description   : .C file function description
 * Version       : 1.0
 * Function List :
 *
 * Record        :
 * 1.Date        : 2018-12-27
 *   Author      : Bruce HW&RD
 *   Modification: Created file

********************************************************************************/

#include <MyProject.h>


MotorTailWindTypeDef xdata  TailWindDetect;

/*---------------------------------------------------------------------------*/
/* Name     :   void TailWindDetectInit(void)
/* Input    :   NO
/* Output   :   NO
/* Description: 顺逆风参数初始化
/*---------------------------------------------------------------------------*/
void TailWindDetectInit(void)
{
    MOE = 0;
    memset(&TailWindDetect, 0, sizeof(MotorTailWindTypeDef));   //  TailWindDetect所有变量清零
    TailWindDetect.MotorTailWindState     = NormalTailWind;     //  初始状态为正常的顺逆风状态
    TailWindDetect.TempThailWindSpeedBase = ThailWindSpeedBase; //  顺逆风计算的speedbase
    FOC_Init();                                                 // FOC的初始化
    FOC_QMIN        = QOUTMIN;
    SetBit(FOC_CR0, ESCMS);
    FOC_DQKP    = DQKP_TailWind;                                // 顺逆风的电流环KP
    FOC_DQKI    = DQKI_TailWind;                                // 顺逆风的电流环KI
    FOC_EKP     = OBSW_KP_GAIN_WIND;                            // 顺逆风速度估算的KP
    FOC_EKI     = OBSW_KI_GAIN_WIND;                            // 顺逆风速度估算的KI
    FOC_OMEKLPF = SPEED_KLPF_WIND;                              // 顺逆风下的速度滤波系数
    SetBit(FOC_CR1, ANGM);                                      // 估算模式
    DRV_CMR |= 0x3F;                                            // U、V、W相输出
    FOC_IDREF = 0;                                              // D轴给定电流
    FOC_IQREF = 0;
    MOE = 1;                                                    // 打开MOE
}

void TailWindSpeedDetect(void)
{
    static int16 LatestTheta;

    if (mcState == mcTailWind)
    {
        if (TailWindDetect.MotorTailWindState == NormalTailWind)
        {
            LatestTheta = FOC__ETHETA;
            if (LatestTheta < -30946)
            {
                if ((TailWindDetect.SpeedTimerClearStatus == 0) || (TailWindDetect.AngleState == 3))
                {
                    TailWindDetect.SpeedCountTimer       = 0;
                    TailWindDetect.SpeedTimerClearStatus = 1;
                    
                    if (TailWindDetect.AngleState == 3)
                    {
                        TailWindDetect.ShakeTimes++;
                    }
                    
                    switch (TailWindDetect.ThetaStep)
                    {
                        case 0:     TailWindDetect.ThetaStep += 1;  break;
                        
                        case 1:     TailWindDetect.ThetaStep += 1;  break;
                        
                        case 4:
                        case 10:
                            TailWindDetect.SpeedStoreNum++;
                            TailWindDetect.ThetaStep = 0;
                            break;
                            
                        default:    TailWindDetect.ThetaStep = 0;  break;
                    }
                }
                TailWindDetect.AngleState = 1;
            }
            else if ((LatestTheta > -1820) && (LatestTheta < 1820)) //<10 >-10
            {
                if ((TailWindDetect.AngleState == 1) || (TailWindDetect.AngleState == 3))
                {
                    TailWindDetect.SpeedCountTimer += 1;//·ÀÖ¹Îª0
                    DivQ_L_MDU(TailWindDetect.TempThailWindSpeedBase >> 16, TailWindDetect.TempThailWindSpeedBase, TailWindDetect.SpeedCountTimer, TailWindDetect.TailWindSpeed);
                    
                    switch (TailWindDetect.ThetaStep)
                    {
                        case 0:     break;
                        case 1:
                        case 2:     TailWindDetect.ThetaStep += 3;  break;
                        default:    TailWindDetect.ThetaStep = 0;  break;
                    }

                    if (TailWindDetect.SpeedTimerClearStatus == 1)
                    {
                        TailWindDetect.SpeedTimerClearStatus = 0;
                    }
                    if (TailWindDetect.AngleState == 1)
                    {
                        TailWindDetect.MotorDir = CW;
                    }
                    else if (TailWindDetect.AngleState == 3)
                    {
                        TailWindDetect.MotorDir = CCW;
                    }
                    TailWindDetect.ShakeTimes = 0;
                }
                TailWindDetect.AngleState = 2;
            }
            else if (LatestTheta > 30946)
            {
                if ((TailWindDetect.SpeedTimerClearStatus == 0) || (TailWindDetect.AngleState == 1))
                {
                    TailWindDetect.SpeedCountTimer       = 0;
                    TailWindDetect.SpeedTimerClearStatus = 1;
                    if (TailWindDetect.AngleState == 1)
                    {
                        TailWindDetect.ShakeTimes++;
                    }
                    switch (TailWindDetect.ThetaStep)
                    {
                        case 0:     TailWindDetect.ThetaStep += 1;  break;
                        case 1:     TailWindDetect.ThetaStep += 1;  break;
                        case 4:
                        case 10:
                            TailWindDetect.SpeedStoreNum++;
                            TailWindDetect.ThetaStep = 0;
                            break;
                        default:    TailWindDetect.ThetaStep = 0;  break;
                    }
                }
                TailWindDetect.AngleState = 3;
            }
        }
    }
}

/*---------------------------------------------------------------------------*/
/* Name     :   void FOCCloseLoopStart(void)
/* Input    :   NO
/* Output   :   NO
/* Description: 顺风启动
/*---------------------------------------------------------------------------*/
void FOCCloseLoopStart(void)
{
    //配置启动的参数，Omega模式
    FOC_EFREQACC  = 6;
    FOC_EFREQMIN  = Motor_Omega_Ramp_Min;
    FOC_EFREQHOLD = Motor_Omega_Ramp_End;
    SetBit(FOC_CR1, EFAE);                         // 估算器强制输出
    ClrBit(FOC_CR1, RFAE);                         // 禁止强拉
    SetBit(FOC_CR1, ANGM);                         // 估算模式
    //电流环的PI和输出限赋值
    FOC_DQKP = DQKPStart;
    FOC_DQKI = DQKI;
    FOC_DMAX = DOUTMAX;
    FOC_DMIN = DOUTMIN;
    FOC_QMAX = QOUTMAX;
    FOC_QMIN = QOUTMIN;
    /*********PLL或SMO**********/
    #if (EstimateAlgorithm == SMO)
    //根据不同转速确启动的ATO_BW值
    //根据不同转速确启动的ATO_BW值
    FOC_EKP               = OBSW_KP_GAIN_RUN4;
    FOC_EKI               = OBSW_KI_GAIN_RUN4;
    mcFocCtrl.OUTLoopCtrl.Close_loop_delay_time = (SpeedloopDeley_Time >> 2);
    mcFocCtrl.mcIqref     = I_Value(0.1);// IQ_RUN_CURRENT;
    mcFocCtrl.State_Count = 1000;
    #elif (EstimateAlgorithm == PLL)
    mcFocCtrl.mcIqref = I_Value(0.4);
    #endif //end    EstimateAlgorithm
    /*estimate parameter set*/
    FOC_OMEKLPF = SPEED_KLPF;
    FOC_IDREF   = ID_RUN_CURRENT;     // D轴启动电流
    FOC_IQREF   = mcFocCtrl.mcIqref;  // Q轴启动电流
    PI3_UKH     = mcFocCtrl.mcIqref;
    mcSpeedRamp.ActualValue = mcFocCtrl.SpeedFlt ;
    mcSpeedRamp.TargetValue = mcSpeedRamp.TargetValue;
    mcState            = mcRun;
    mcFocCtrl.CtrlMode = 1;
}

/*---------------------------------------------------------------------------*/
/* Name     :   void FOCTailWindTimeLimit(void)
/* Input    :   NO
/* Output   :   NO
/* Description: FOC顺逆风检测的时间处理
/*---------------------------------------------------------------------------*/
void FOCTailWindTimeLimit(void)
{
    if (mcState == mcTailWind)
    {
        //1Hz Min
        if (TailWindDetect.SpeedCountTimer < TailWind_Time) // SpeedCountTimer-计算时间间隔
        {
            TailWindDetect.SpeedCountTimer++;
        }
        else
        {
            TailWindDetect.TailWindSpeed       = 0; //速度为0
            TailWindDetect.SpeedCountTimer     = 0;
            TailWindDetect.SpeedOverFlowStatus = 1; // 转速计数溢出标志
        }
    }
}
/*---------------------------------------------------------------------------*/
/* Name     :   void TailWindDealwith(void)
/* Input    :   NO
/* Output   :   NO
/* Description: 顺风逆风参数配置函数
/*---------------------------------------------------------------------------*/
void FOCTailWindDealwith(void)
{
    /*********PLL或SMO**********/
    #if (EstimateAlgorithm == SMO)
    SMO_TailWindDealwith();
    #elif (EstimateAlgorithm == PLL)
    PLL_TailWindDealwith();
    #endif //end    EstimateAlgorithm
}

void SMO_TailWindDealwith(void)
{
    //电机状态为正转且转速计数溢出时；或1-3状态抖动次数大于2次时，认为静止
    if ((TailWindDetect.SpeedOverFlowStatus || (TailWindDetect.ShakeTimes > 2) || (TailWindDetect.TailWindBrakeCount > 3)))
    {
        if (TailWindDetect.BrakeCurrentDetectStatus == 0)
        {
            TailWindDetect.TailWindBrakeStatus = 1;
            Break_delay_time = SPEED_BREAK_TIME0;
            TailWindDetect.BrakeRelaxTime = 1000;//3800;
        }
        else
        {
            MOE = 0;
            ClrBit(DRV_CR, FOCEN);    //关闭FOC
            mcFocCtrl.State_Count              = 10;
            mcState                           = mcPosiCheck;
            McStaSet.SetFlag.PosiCheckSetFlag = 0;
            mcFocCtrl.mcPosCheckAngle         = 0xffff;         // 角度赋初值
            TailWindDetect.CWFlage = 0;
        }
    }
    
    if (TailWindDetect.MotorDir == CW)
    {
        //      SetBuzzer(1,2,1);
        //超过检测次数TailWindDetect.SpeedStoreNum切转速TailWindStartMinSpeed以上时直接顺风启动
        if ((TailWindDetect.TailWindSpeed < 40) && (TailWindDetect.SpeedStoreNum > 6)) // TailWindDetect.SpeedStoreNum>0
        {
            MOE = 0;
            FOC_CR1 = 0x00;
            ClrBit(DRV_CR, FOCEN);
            DRV_DR  = DRV_ARR + 1;
            DRV_CMR &= 0xFFC0;
            DRV_CMR |= 0x015;            // 三相下桥臂通，刹车
            ClrBit(DRV_CR, OCS);         // OCS = 0, DRV_COMR;OCS = 1, FOC/SVPWM/SPWM
            MOE = 1;
            mcFocCtrl.State_Count = 2300;  //2300
        }
        else if ((TailWindDetect.TailWindSpeed > 450) && (TailWindDetect.SpeedStoreNum > 0))
        {
            MOE = 0;
            FOC_CR1 = 0x00;
            ClrBit(DRV_CR, FOCEN);
            DRV_DR  = DRV_ARR + 1;
            DRV_CMR &= 0xFFC0;
            DRV_CMR |= 0x015;            // 三相下桥臂通，刹车
            ClrBit(DRV_CR, OCS);         // OCS = 0, DRV_COMR;OCS = 1, FOC/SVPWM/SPWM
            MOE = 1;
            mcFocCtrl.State_Count = 1000;  //1000
        }
        else if ((TailWindDetect.TailWindSpeed >= 40) && (TailWindDetect.TailWindSpeed < 450) && (TailWindDetect.SpeedStoreNum > 3))
        {
            LowSpeedCtrl.LowSpeedFlag = 1;
            FOCCloseLoopStart();
        }
    }
    else if ((TailWindDetect.MotorDir == CCW) && (TailWindDetect.SpeedStoreNum > 10)) //||((TailWindDetect.MotorDir == CW)&&(TailWindDetect.TailWindSpeed>TailWindStartMinSpeed)))
    {
        MOE = 0;
        FOC_CR1 = 0x00;
        ClrBit(DRV_CR, FOCEN);
        DRV_DR  = 0.98 * DRV_ARR;
        DRV_CMR &= 0xFFC0;
        DRV_CMR |= 0x015;            // 三相下桥臂通，刹车
        ClrBit(DRV_CR, OCS);         // OCS = 0, DRV_COMR;OCS = 1, FOC/SVPWM/SPWM
        MOE = 1;
        McStaSet.SetFlag.TailWindSetFlag  = 0;
        
        if (TailWindDetect.TailWindSpeed > 180)
        {
            mcFocCtrl.State_Count = 12000;
            //            TailWindDetect.AntiFlag = 2;
        }
        else if (TailWindDetect.TailWindSpeed > 150)
        {
            mcFocCtrl.State_Count = 10500;
            //            TailWindDetect.AntiFlag = 3;
        }
        else if (TailWindDetect.TailWindSpeed > 60)
        {
            mcFocCtrl.State_Count = 9500;
            //            TailWindDetect.AntiFlag = 4;
        }
        else if (TailWindDetect.TailWindSpeed > 30)
        {
            mcFocCtrl.State_Count = 8500;
            //            TailWindDetect.AntiFlag = 5;
        }
        else if (TailWindDetect.TailWindSpeed > 0)
        {
            mcFocCtrl.State_Count = 6000;
            //            TailWindDetect.AntiFlag = 6;
        }
        else if (TailWindDetect.TailWindSpeed == 0)
        {
            mcFocCtrl.State_Count = 5500;
            //            TailWindDetect.AntiFlag = 7;
        }
        
        TailWindDetect.TailWindBrakeCount++;
        //           }
    }
}
